#include "Sensor.h"
#include "car_init.h"
#include "math.h"

float AD_value[5] = {0};
float ad0_one = 0;
float ad1_one= 0;
float ad2_one= 0;
float ad3_one= 0;
float ad4_one= 0;
float dev_new= 0;
float dev_dev= 0;
float e = 0,ec = 0;
void read_adc()
{
	int16 ad_value[5][9],temp;
  int16 i,j;
  int16 k1,temp1;
  
  for(i = 0;i < 9;i++)
  {
    ad_value[0][i] = adc_convert(ADC_1,ADC1_CH4_B15);
    ad_value[1][i] = adc_convert(ADC_1,ADC1_CH5_B16);
    ad_value[2][i] = adc_convert(ADC_1,ADC1_CH6_B17);
    ad_value[3][i] = adc_convert(ADC_1,ADC1_CH3_B14);
    ad_value[4][i] = adc_convert(ADC_1,ADC1_CH7_B18);
    
  }
  
  for(i = 0;i < 5;i++) //5???
  {
    for(j = 0;j < 8;j++)
    {
      for(k1 = 0;k1 < 8-j;k1++)
      {
        temp1 = ad_value[i][k1] - ad_value[i][k1+1];
        if(temp1 < 0)
        {
          temp1 = temp1 * (-1);
        }
        else
        {
          temp1 = temp1 * (1);
        }
        if(temp1 < 5)
          ad_value[i][k1+1] = ad_value[i][k1];
      }
    }
  }
  for(i = 0;i < 5;i++) 
  {
    for(j = 0;j < 8;j++)
    {
      for(k1 = 0;k1 < 8-j;k1++)
      {
        if(ad_value[i][k1] > ad_value[i][k1+1])
        {
          temp = ad_value[i][k1+1];
          ad_value[i][k1+1] = ad_value[i][k1];
          ad_value[i][k1] = temp;
        }
      }
    }
  }
  for(i = 0;i < 5;i++)
  {
    AD_value[i] = ad_value[i][1] + ad_value[i][2] + ad_value[i][3]+ ad_value[i][4] + ad_value[i][5] + ad_value[i][6] + ad_value[i][7];
    AD_value[i] = (float)AD_value[i] / 7;
  }
}

void sensor_to_one()
{
	float ad0 = 0;//????,????
  float ad1 = 0;
  float ad2 = 0;
  float ad3 = 0;
  float ad4 = 0;
  float ad_min[5] = {10,10,10,10,10};
  float ad_max[5] = {960,960,960,960,960};
  ad0 = AD_value[0];
  ad1 = AD_value[1];
  ad2 = AD_value[2];
  ad3 = AD_value[3];
  ad4 = AD_value[4];
  
  ad0_one = 255 * (ad0 - ad_min[0]) / (ad_max[0] - ad_min[0]);
  if(ad0_one <= 0)//??
  {
    ad0_one = 0;
  }
  if(ad0_one >= 255)
  {
    ad0_one = 255;
  }
  ad1_one = 255 * (ad1 - ad_min[0]) / (ad_max[0] - ad_min[0]);
  if(ad1_one <= 0)//??
  {
    ad1_one = 0;
  }
	if(ad0_one >= 255)
  {
    ad0_one = 255;
  }
  ad2_one = 255 * (ad2 - ad_min[0]) / (ad_max[0] - ad_min[0]);
  if(ad2_one <= 0)//??
  {
    ad2_one = 0;
  }
	if(ad2_one >= 255)
  {
    ad2_one = 255;
  }
  ad3_one = 255 * (ad3 - ad_min[0]) / (ad_max[0] - ad_min[0]);
  if(ad3_one <= 0)//??
  {
    ad3_one = 0;
  }
	if(ad3_one >= 255)
  {
    ad3_one = 255;
  }
  ad4_one = 255 * (ad4 - ad_min[0]) / (ad_max[0] - ad_min[0]);
  if(ad4_one <= 0)//??
  {
    ad4_one = 0;
  } 
  if(ad4_one >= 255)
  {
    ad4_one = 255;
  }	
}
void error_calculate(void)
{
  static float dev[5];
	float error_error;
  float weight,parallel_error,vertical_error;
  ///?????????
  parallel_error = ((float)(sqrt(ad1_one) - sqrt(ad2_one))/(float)(ad1_one + ad2_one) -  (float)(sqrt(ad1_one) - sqrt(ad0_one))/(float)(ad1_one + ad0_one));
  vertical_error = (float)(sqrt(ad1_one) - sqrt(ad4_one))/(float)(ad1_one + ad4_one) -  (float)(sqrt(ad1_one) - sqrt(ad3_one))/(float)(ad1_one + ad3_one);
  weight = fabs(vertical_error);
  //????????
  
  dev[4] = dev[3];
  dev[3] = dev[2];
  dev[2] = dev[1];
  dev[1] = dev[0];
  dev[0] = (parallel_error * (1 - weight) + vertical_error * weight) * 1000;
	dev_dev = dev[0] - dev[4];
  dev_new = dev[0];
	
	error_error = (ad3_one - ad4_one)/(ad3_one + ad4_one);
  
	e = fabs(dev_new);
  ec = fabs(error_error);
}